﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Collections;
using SlimMath;

/**
 * k-DOPの構築、更新は
 * 13個のOrientationはM3Gの定義より全て+方向で定義している。
 * 従ってMaxMinは前半13個がMax値で、後半13個がMin値でなければならない
 * (だから名前もMinMaxではなくMaxMinになっている)
 * 
 * */
namespace M3G {
    public class CollisionShape {
        readonly Vector3[] normals = new Vector3[13]{new Vector3 ( 1, 0, 0),  // +方向13軸
                                                     new Vector3 ( 0, 0, 1),
                                                     new Vector3 ( 0, 1, 0),
                                                     new Vector3 ( 1, 0, 1) / (float)Math.Sqrt(2),
                                                     new Vector3 ( 1, 0,-1) / (float)Math.Sqrt(2),
                                                     new Vector3 ( 1, 1, 0) / (float)Math.Sqrt(2),
                                                     new Vector3 ( 1,-1, 0) / (float)Math.Sqrt(2),
                                                     new Vector3 ( 0, 1, 1) / (float)Math.Sqrt(2),
                                                     new Vector3 ( 0, 1,-1) / (float)Math.Sqrt(2),
                                                     new Vector3 ( 1, 1, 1) / (float)Math.Sqrt(3),
                                                     new Vector3 ( 1, 1,-1) / (float)Math.Sqrt(3),
                                                     new Vector3 ( 1,-1, 1) / (float)Math.Sqrt(3),
                                                     new Vector3 ( 1,-1,-1) / (float)Math.Sqrt(3)};
        #region Field
        BitArray orientations;  // 最大26bit
        float[]   maxMin;       // 最大26個(max[13] + min[13])
        float[]   envelope;     // 不定
        #endregion


        public CollisionShape () {
            this.orientations = new BitArray (26);
            this.maxMin = new float[26];
            this.envelope = null;
        }

        public CollisionShape (CollisionShape another) {
            this.orientations = another.orientations;
            this.maxMin = another.Max.Concat (another.Min).ToArray ();
            this.envelope = another.Envelope;
        }

        public int Rank {
            get {
                var rank = 0;
                for (var i=0; i < 13; i++) {
                    if (orientations[i]) {
                        rank = i + 1;
                    }
                }
                return rank;
            }
        }

        public int Orientations {
            get {
                var ori = 0;
                for (var i=0; i < 13; i++) {
                    if (orientations[i]) {
                        ori |= 1 << i;
                    }
                }
                return ori;
            }
        }

        public float[] Min {
            get {
                var min = new float[13];
                Array.Copy (maxMin, 13, min, 0, 13);
                return min;
            }
        }

        public float[] Max {
            get {
                var max = new float[13];
                Array.Copy (maxMin, 0, max, 0, 13);
                return max;
            }
        }



        public void ConstructKDOP (int orientations, float[] verticesXYZ) {
            if (verticesXYZ == null) {
                throw new ArgumentNullException ("Vertices is null");
            }
            if (verticesXYZ.Length % 3 != 0) {
                throw new ArgumentException ("Length of verticesXYZ must be x3");
            }


            var vertexCount = (verticesXYZ != null) ? verticesXYZ.Length / 3 : 0;
            var vertices = new List<Vector3> ();
            for (var v=0; v < vertexCount; v++) {
                vertices.Add (new Vector3 (verticesXYZ[v * 3], verticesXYZ[v * 3 + 1], verticesXYZ[v * 3 + 2]));
            }

            this.orientations.SetAll (false);

            for (int i=0; i < 13; i++) {
                if ((orientations & 1 << i) != 0) {
                    var min = float.MaxValue;
                    var max = float.MinValue;
                    for (var v=0; v < vertexCount; v++) {
                        var projectedValue = Vector3.Dot (vertices[v], normals[i]);
                        min = (projectedValue < min) ? projectedValue : min;
                        max = (projectedValue > max) ? projectedValue : max;
                    }
                    this.orientations[i] = true;
                    this.orientations[i + 13] = true;
                    this.maxMin[i + 0] = max / normals[i].Length;
                    this.maxMin[i + 13] = min / normals[i].Length;
                }
            }
        }

        public void SetKDOP (int orientations, float[] min, float[] max) {
            if (orientations != 0 && (min == null || max == null)) {
                throw new ArgumentNullException ("Min or Max is null");
            }

            this.orientations.SetAll (false);

            for (int i=0; i < 13; i++) {
                if ((orientations & 1 << i) != 0) {
                    this.orientations[i] = true;
                    this.orientations[i + 13] = true;
                    this.maxMin[i + 0] = max[i];
                    this.maxMin[i + 13] = min[i];
                }
            }
        }


        Vector3? Intersect3Plane (Vector3 n1, float d1, Vector3 n2, float d2, Vector3 n3, float d3) {
            var denom = Vector3.Dot (n1, Vector3.Cross (n2, n3));
            if (Math.Abs (denom) < Mathf.Epsilon) {
                return null;
            }
            return (d1 * Vector3.Cross (n2, n3) + Vector3.Cross (n1, (d3 * n2 - d2 * n3))) / denom;
        }

        public void GenerateEnvelope () {
            var vertices = new List<Vector3> ();

            for (var i=0; i < 26; i++) {
                if (orientations[i]) {
                    for (var j=i; j < 26; j++) {
                        if (orientations[j]) {
                            for (var k=j; k < 26; k++) {
                                if (orientations[k]) {
                                    var si = (i < 13) ? 1 : -1;
                                    var sj = (j < 13) ? 1 : -1;
                                    var sk = (k < 13) ? 1 : -1;
                                    var point = Intersect3Plane (si * normals[i % 13], si * maxMin[i],
                                                                 sj * normals[j % 13], sj * maxMin[j],
                                                                 sk * normals[k % 13], sk * maxMin[k]);
                                    if (point != null) {
                                        vertices.Add ((Vector3)point);
                                    }
                                }
                            }
                        }
                    }
                }
            }

            vertices.RemoveAll (vertex => {
                for (var i=0; i < 13; i++) {
                    if (orientations[i]) {
                        var projectedValue = Vector3.Dot (vertex, normals[i]);
                        if (projectedValue < maxMin[i + 13] - Mathf.Epsilon || projectedValue > maxMin[i + 0] + Mathf.Epsilon) {
                            return true;
                        }
                    }
                }
                return false;
            });

            this.envelope = new float[vertices.Count * 3];
            for (var i=0; i < vertices.Count; i++) {
                this.envelope[i * 3 + 0] = vertices[i][0];
                this.envelope[i * 3 + 1] = vertices[i][1];
                this.envelope[i * 3 + 2] = vertices[i][2];
            }

        }

        public float[] Envelope {
            get { return (float[])envelope; }
        }

        /**
         * このコリジョン形状を指定のTransformで変換します。
         * エンベロープは古いままなので必要ならGenerateEnvelop()を読んで再計算します。
         * */
        public void Transform (Transform transform) {
            var m = transform.Matrix;
            if (!(m[12] == 0 && m[13] == 0 && m[14] == 0 && m[15] == 1)) {
                throw new ArgumentException ("CollisionShape must be transformed by linear(affine) tranform");
            }
            if (envelope == null) {
                throw new ArgumentException ("Envelope is null, try BuildEnvelop");
            }

            transform.TransformVectors3 (envelope, true);

            // x,y,z,w --> x,y,z
            ConstructKDOP (Orientations, envelope);
        }

        public bool Intersect (CollisionShape another) {
            if (another == null) {
                throw new ArgumentNullException ("Another CollisionShape is null");
            }
            for (var i=0; i < 13; i++) {
                if (maxMin[i] < another.maxMin[i + 13] || maxMin[i + 13] > another.maxMin[i]) {
                    return false;
                }
            }

            return true;
        }

        /**
         * このコリジョン形状に指定のCollisionShapeを合成します。
         * エンベロープは古いままなので必要ならGenerateEnvelop()を読んで再計算します。
         * */
        public void Merge (CollisionShape another) {
            if (another == null) {
                throw new ArgumentNullException ("Another CollsionShpe is null");
            }
            for (var i=0; i < 13; i++) {
                if (orientations[i] && !another.orientations[i]) {
                    throw new ArgumentException ("Orientations of Another CollisionShape is not compatible with orientations of this CollisionShape");
                }
            }

            for (var i=0; i < 26; i++) {
                if (orientations[i] && i < 13) {
                    this.maxMin[i] = Math.Max (this.maxMin[i], another.maxMin[i]);
                }
                if (orientations[i] && i >= 13) {
                    this.maxMin[i] = Math.Min (this.maxMin[i], another.maxMin[i]);
                }
            }

        }
    }
}
